#include <iostream>
#include <string>
#include <vector>

#include <ros/ros.h>
#include<std_msgs/Float64MultiArray.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud2_iterator.h>
#include <sensor_msgs/NavSatFix.h>
#include <token_msgs/Nav_ori.h>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/common/common.h>
#include <pcl/features/moment_of_inertia_estimation.h>

#include <Eigen/Core>

typedef pcl::PointXYZI PointType;

class CalibPclImg
{
public:
    CalibPclImg();
    ~CalibPclImg(){};

    void pcl2img(const sensor_msgs::PointCloud2ConstPtr& ptmsg);
    void ObjCallback(const std_msgs::Float64MultiArray::ConstPtr& bboxmsg);
    void NavCallback(const token_msgs::Nav_ori::ConstPtr& navmsg);

    ros::NodeHandle nh;
    ros::Subscriber subPointCloud;
    ros::Subscriber subObj;
    ros::Subscriber subGPS;

    Eigen::Matrix<double,4,4> transfromEigentoCamera;
    Eigen::Matrix<double,4,4> cameraPara;
    Eigen::Vector4d thispoint;
    PointType centerPt;
    pcl::PointCloud<PointType> pcl_cloud_;

    token_msgs::Nav_ori nav_info;

    float m2l = 0.000008983;
    int img_w = 1600;
    int img_h = 1200;
};